/*
 * joint_states_publisher.cpp
 *
 *  Created on: Aug 3, 2011
 *      Author: zhaofei
 */
#include <map>
#include <math.h>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetPositionIK.h>
#include <tf/transform_broadcaster.h>


class JointStatesPublisher{

public:
	JointStatesPublisher()
	{
		ros::service::waitForService("arm_kinematics/get_ik_solver_info");
		ros::service::waitForService("arm_kinematics/get_ik");
		query_client = nh_.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("arm_kinematics/get_ik_solver_info");
		ik_client = nh_.serviceClient<kinematics_msgs::GetPositionIK>("arm_kinematics/get_ik");
		joint_state_publisher_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);

		if (query_client.call(request, response))
		{
			for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++)
			{
				ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
				joint_iter_.insert(make_pair(response.kinematic_solver_info.joint_names[i], i));
				joint_value_.insert(make_pair(response.kinematic_solver_info.joint_names[i], 0.0));
			}
		}
		else
		{
			ROS_ERROR("Could not call query service");
			ros::shutdown();
			exit(1);
		}

		gpik_req.timeout = ros::Duration(5.0);
		gpik_req.ik_request.ik_link_name = "left_arm_link_7";
		gpik_req.ik_request.pose_stamped.header.frame_id = "arm_reference_link";

	}

	bool computeIK(double px, double py, double pz, double ox, double oy, double oz)
	{

		ROS_INFO("px = %f", px);
		ROS_INFO("py = %f", py);
		ROS_INFO("pz = %f", pz);
		ROS_INFO("ox = %f", ox);
		ROS_INFO("oy = %f", oy);
		ROS_INFO("oz = %f", oz);

		tf::Quaternion q = tf::createQuaternionFromRPY(ox,oy,oz);

		gpik_req.ik_request.pose_stamped.pose.position.x = px;
		gpik_req.ik_request.pose_stamped.pose.position.y = py;
		gpik_req.ik_request.pose_stamped.pose.position.z = pz;

		gpik_req.ik_request.pose_stamped.pose.orientation.x = q.getX();
		gpik_req.ik_request.pose_stamped.pose.orientation.y = q.getY();
		gpik_req.ik_request.pose_stamped.pose.orientation.z = q.getZ();
		gpik_req.ik_request.pose_stamped.pose.orientation.w = q.getW();

		gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
		gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;

		for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++)
		{
			gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position
																		 + response.kinematic_solver_info.limits[i].max_position) / 2.0;
		}

		if (ik_client.call(gpik_req, gpik_res))
		{
			if (gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
			{
				for (unsigned int i = 0; i < gpik_res.solution.joint_state.name.size(); i++)
				{
					ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
				}
				joint_state_ = gpik_res.solution.joint_state;
			}
			else
			{
				ROS_ERROR("Inverse kinematics failed");
				return false;
			}
		}
		else
		{
			ROS_ERROR("Inverse kinematics service call failed");
			return false;
		}

		return true;
	}

	bool computeIKQuaternion(double px, double py, double pz, double ox, double oy, double oz, double ow)
	{
		ros::service::waitForService("arm_kinematics/get_ik");

		gpik_req.ik_request.pose_stamped.pose.position.x = px;
		gpik_req.ik_request.pose_stamped.pose.position.y = py;
		gpik_req.ik_request.pose_stamped.pose.position.z = pz;

		gpik_req.ik_request.pose_stamped.pose.orientation.x = ox;
		gpik_req.ik_request.pose_stamped.pose.orientation.y = oy;
		gpik_req.ik_request.pose_stamped.pose.orientation.z = oz;
		gpik_req.ik_request.pose_stamped.pose.orientation.w = ow;

		gpik_req.ik_request.ik_seed_state.joint_state.position.resize(
				response.kinematic_solver_info.joint_names.size());
		gpik_req.ik_request.ik_seed_state.joint_state.name =
				response.kinematic_solver_info.joint_names;

		for (unsigned int i = 0;
				i < response.kinematic_solver_info.joint_names.size(); i++)
				{
			gpik_req.ik_request.ik_seed_state.joint_state.position[i] =
					(response.kinematic_solver_info.limits[i].min_position
							+ response.kinematic_solver_info.limits[i].max_position)
							/ 2.0;
		}

		if (ik_client.call(gpik_req, gpik_res))
		{
			if (gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
			{
				for (unsigned int i = 0;
						i < gpik_res.solution.joint_state.name.size(); i++)
						{
					ROS_INFO(
							"Joint: %s %f", gpik_res.solution.joint_state.name[i].c_str(), gpik_res.solution.joint_state.position[i]);

					//joint_iter_[gpik_res.solution.joint_state.name[i]] = i;
					//joint_value_[gpik_res.solution.joint_state.name[i]] = gpik_res.solution.joint_state.position[i];

				}
				joint_state_ = gpik_res.solution.joint_state;
			}
			else
			{
				ROS_ERROR("Inverse kinematics failed");
				return false;
			}
		}
		else
		{
			ROS_ERROR("Inverse kinematics service call failed");
			return false;
		}

		return true;
	}


	bool publishJoinStates()
	{
		joint_state_publisher_.publish(joint_state_);
		return true;
	}

private:

	ros::NodeHandle nh_;

	ros::Publisher joint_state_publisher_;
	ros::ServiceClient query_client;
	ros::ServiceClient ik_client;

	// define the service messages
	kinematics_msgs::GetKinematicSolverInfo::Request request;
	kinematics_msgs::GetKinematicSolverInfo::Response response;

	// define the service messages
	kinematics_msgs::GetPositionIK::Request gpik_req;
	kinematics_msgs::GetPositionIK::Response gpik_res;

	std::map<std::string, unsigned int> joint_iter_;
	std::map<std::string, double> joint_value_;
	sensor_msgs::JointState joint_state_;

};

int main(int argc, char **argv)
{
	ros::init(argc, argv, "joint_states_publisher");
	ros::NodeHandle nh;
	ros::NodeHandle param_nh("~");

	double px_, py_, pz_, ox_, oy_, oz_, ow_;
	bool use_q_ = true;
	param_nh.param("px", px_, 0.0);
	param_nh.param("py", py_, 0.0);
	param_nh.param("pz", pz_, 0.0);
	param_nh.param("ox", ox_, 0.0);
	param_nh.param("oy", oy_, 0.0);
	param_nh.param("oz", oz_, 0.0);
	param_nh.param("ow", ow_, 1.0);
	param_nh.param("use_q", use_q_, false);



	JointStatesPublisher joint_states_pub;


	ROS_INFO("use_quaternion: %i", use_q_);
	if(use_q_)
	{
		if (!joint_states_pub.computeIKQuaternion(px_, py_, pz_, ox_, oy_, oz_, ow_))
		{
			ROS_ERROR("use quaternion computeIK failed");
		}
	}
	else
	{
		if (!joint_states_pub.computeIK(px_, py_, pz_, ox_, oy_, oz_))
		{
			ROS_ERROR("use RPY computeIK failed");

		}
	}

	ros::Rate pub_rate(5.0);
	while(ros::ok())
	{
		joint_states_pub.publishJoinStates();
	}

	ros::shutdown();
	return 0;
}
